#include "mainwindow.h"
#include <QDebug>

void ImuDataThread::run()
{
    imu_data_node_cpp = rclcpp::Node::make_shared("imu_data_");
    imu_data_sub = imu_data_node_cpp->create_subscription<
        std_msgs::msg::Float64MultiArray>(
        "imu_data_pub",
        10,
        std::bind(&ImuDataThread::imu_data_cb,this,std::placeholders::_1));
    rclcpp::spin(imu_data_node_cpp);
}
void ImuDataThread::imu_data_cb(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
{
    // 获取RPY值
    double roll = msg->data[0];
    double pitch = msg->data[1] + 90;
    double yaw = msg->data[2];
    // 使用QDebug输出这些值
    // qDebug() << "Roll:" << roll << "Pitch:" << pitch << "Yaw:" << yaw;
    // 通过信号发送出线程
    emit imuEmitData(roll,pitch,yaw);
}
